package gnss

import (
	"bytes"
	"dgo/goutils/utils"
	"encoding/binary"
	"fmt"
	"gitee.com/ymofen/gobase"
	"math"
	"sort"
)

const (
	PS_INVALID_VALUE = -16384
	PH_INVALID_VALUE = -2097152

	//PS_ZERO_VALUE    = 16384
)

type RtcmBuf []byte

const (
	rtcm_satid_gps_start = 1
	rtcm_satid_gps_max   = 40
	rtcm_satid_gal_start = rtcm_satid_gps_start + rtcm_satid_gps_max
	rtcm_satid_gal_max   = 40
	rtcm_satid_glo_start = rtcm_satid_gal_start + rtcm_satid_gal_max
	rtcm_satid_glo_max   = 40
	rtcm_satid_bds_start = rtcm_satid_glo_start + rtcm_satid_glo_max
	rtcm_satid_bds_max   = 70
	rtcm_satid_qzs_start = rtcm_satid_bds_start + rtcm_satid_bds_max
	rtcm_satid_qzs_max   = 40
	rtcm_satid_sbs_start = rtcm_satid_qzs_start + rtcm_satid_qzs_max
	rtcm_satid_sbs_max   = 40

	rtcm_satid_max = rtcm_satid_sbs_start + rtcm_satid_sbs_max
)

func IsWholeRtcmBuf(buf []byte) bool {
	l := len(buf)
	if l < 6 {
		return false
	}

	msgl := binary.BigEndian.Uint16(buf[1:]) // msg len
	msgl = uint16(int(msgl) & 0x3FF)         // 0x3FF 1111111111  1023
	if msgl != uint16(l)-6 {
		return false

	}

	crc1 := utils.CRC24QBuf(buf[0 : l-3])
	crc := GetBitU(buf[l-3:], 0, 24)
	return crc1 == crc
}

func RtcmSatId2SatIdx(satsys byte, satid uint16) uint16 {
	switch satsys {
	case SYS_GPS:
		return rtcm_satid_gps_start + satid - 1
	case SYS_BD:
		return rtcm_satid_bds_start + satid - 1
	case SYS_GLO:
		return rtcm_satid_glo_start + satid - 1
	case SYS_GAL:
		return rtcm_satid_gal_start + satid - 1
	case SYS_QZS:
		return rtcm_satid_qzs_start + satid - 1
	case SYS_SBS:
		return rtcm_satid_sbs_start + satid - 1
	}
	return 0
}

/*
最大 rtcm_satid_max
*/
func RtcmSatIdFromSatIdx(idx uint16) (satsys byte, satid uint16) {
	if idx > rtcm_satid_sbs_start+rtcm_satid_sbs_max {
		return SYS_UNKOWN, 0
	}

	if idx >= rtcm_satid_sbs_start {
		return SYS_SBS, idx - rtcm_satid_sbs_start + 1
	}

	if idx >= rtcm_satid_qzs_start {
		return SYS_QZS, idx - rtcm_satid_qzs_start + 1
	}

	if idx >= rtcm_satid_gal_start {
		return SYS_GAL, idx - rtcm_satid_gal_start + 1
	}

	if idx >= rtcm_satid_glo_start {
		return SYS_GLO, idx - rtcm_satid_glo_start + 1
	}

	if idx >= rtcm_satid_bds_start {
		return SYS_BD, idx - rtcm_satid_bds_start + 1
	}

	if idx >= rtcm_satid_gps_start {
		return SYS_GPS, idx - rtcm_satid_gps_start + 1
	}

	return SYS_UNKOWN, 0
}

/*
sigid : 1~32

最大 rtcm_satid_max * 32 + 32
*/
func RtcmSignalId2SignalIdx(satsys byte, satid uint16, sig byte) int {
	// x32
	satidx := RtcmSatId2SatIdx(satsys, satid)
	idx := int(satidx)<<5 + int(sig) - 1
	return idx
}

func RtcmSignalIdFromSignalIdx(idx int) (satsys byte, satid uint16, sig byte) {
	idx0 := idx >> 5
	sig = byte(idx-idx0) + 1
	satsys, satid = RtcmSatIdFromSatIdx(uint16(idx0))
	return
}

/*
*
Table 3.5-74 GNSS Phaserange Lock Time Indicator (DF402)
Indicator (i) Minimum Lock Time (ms) Range of Indicated Lock Times (t) (ms)
0 		0  			0 ≤ t < 32
1 		32 			32 ≤ t < 64
2 		64 			64 ≤ t < 128
3 		128 		128 ≤ t < 256
4 		256 		256 ≤ t < 512
5 		512 		512 ≤ t < 1024
6 		1024 		1024 ≤ t < 2048
7 		2048 		2048 ≤ t < 4096
8 		4096 		4096 ≤ t < 8192
9 		8192 		8192 ≤ t < 16384
10 		16384 		16384 ≤ t < 32768
11 		32768 		32768 ≤ t < 65536
12 		65536 		65536 ≤ t < 131072
13 		131072 		131072 ≤ t < 262144
14 		262144 		262144 ≤ t < 524288
15 		524288 		524288 ≤ t
*/
func LockTime2Indicator(ms int64) uint8 {
	if ms < 32 {
		return 0
	} else if ms < 64 {
		return 1
	} else if ms < 128 {
		return 2
	} else if ms < 256 {
		return 3
	} else if ms < 512 {
		return 4
	} else if ms < 1024 {
		return 5
	} else if ms < 2048 {
		return 6
	} else if ms < 4096 {
		return 7
	} else if ms < 8192 {
		return 8
	} else if ms < 16384 {
		return 9
	} else if ms < 32768 {
		return 10
	} else if ms < 65536 {
		return 11
	} else if ms < 131072 {
		return 12
	} else if ms < 262144 {
		return 13
	} else if ms < 524288 {
		return 14
	} else {
		return 15
	}
}

func LockIndicator2Ms(v uint8) int64 {
	switch v {
	case 0:
		return 0
	case 1:
		return 64 - 1
	case 2:
		return 128 - 1
	case 3:
		return 256 - 1
	case 4:
		return 512 - 1
	case 5:
		return 1024 - 1
	case 6:
		return 2048 - 1
	case 7:
		return 4096 - 1
	case 8:
		return 8192 - 1
	case 9:
		return 16384 - 1
	case 10:
		return 32768 - 1
	case 11:
		return 65536 - 1
	case 12:
		return 131072 - 1
	case 13:
		return 262144 - 1
	case 14:
		return 524288 - 1

	}
	return 524288 + 1
}

func LockTime2IndicatorMsm7(v int64) uint16 {
	if v < 64 {
		return uint16(v)
	} else if v < 128 {
		return uint16((v + 64) / 2)
	} else if v < 256 {
		return uint16((v + 256) / 4)
	} else if v < 512 {
		return uint16((v + 768) / 8)
	} else if v < 1024 {
		return uint16((v + 2048) / 16)
	} else if v < 2048 {
		return uint16((v + 5120) / 32)
	} else if v < 4096 {
		return uint16((v + 12288) / 64)
	} else if v < 8192 {
		return uint16((v + 28672) / 128)
	} else if v < 16384 {
		return uint16((v + 65536) / 256)
	} else if v < 32768 {
		return uint16((v + 147456) / 512)
	} else if v < 65536 {
		return uint16((v + 327680) / 1024)
	} else if v < 131072 {
		return uint16((v + 720896) / 2048)
	} else if v < 262144 {
		return uint16((v + 1572864) / 4096)
	} else if v < 524288 {
		return uint16((v + 3407872) / 8192)
	} else if v < 1048576 {
		return uint16((v + 7340032) / 16384)
	} else if v < 2097152 {
		return uint16((v + 15728640) / 32768)
	} else if v < 4194304 {
		return uint16((v + 33554432) / 65536)
	} else if v < 8388608 {
		return uint16((v + 71303168) / 131072)
	} else if v < 16777216 {
		return uint16((v + 150994944) / 262144)
	} else if v < 33554432 {
		return uint16((v + 318767104) / 524288)
	} else if v < 67108864 {
		return uint16((v + 671088640) / 1048576)
	} else if v >= 67108864 {
		return 704
	}
	return 0
}

func LockIndicator2MsMsm7(v uint16) int64 {
	v64 := int64(v)
	if v64 < 63 {
		return v64
	} else if v64 <= 95 {
		return v64*2 - 64
	} else if v64 <= 127 {
		return v64*4 - 256
	} else if v64 <= 159 {
		return v64*8 - 768
	} else if v64 <= 191 {
		return v64*16 - 2048
	} else if v64 <= 223 {
		return v64*32 - 5120
	} else if v64 <= 255 {
		return v64*64 - 12288
	} else if v64 <= 287 {
		return v64*128 - 28672
	} else if v64 <= 319 {
		return v64*256 - 65536
	} else if v64 <= 351 {
		return v64*512 - 147456
	} else if v64 <= 383 {
		return v64*1024 - 327680
	} else if v64 <= 415 {
		return v64*2048 - 720896
	} else if v64 <= 447 {
		return v64*4096 - 1572864
	} else if v64 <= 479 {
		return v64*8192 - 3407872
	} else if v64 <= 511 {
		return v64*16384 - 7340032
	} else if v64 <= 543 {
		return v64*32768 - 15728640
	} else if v64 <= 575 {
		return v64*65536 - 33554432
	} else if v64 <= 607 {
		return v64*131072 - 71303168
	} else if v64 <= 639 {
		return v64*262144 - 150994944
	} else if v64 <= 671 {
		return v64*524288 - 318767104
	} else if v64 <= 703 {
		return v64*1048576 - 671088640
	} else if v64 == 704 {
		return v64*2097152 - 1409286144
	} else {
		return 0
	}
}

// 根据星历计算卫星位置
func CalcuSattileXYZByEph(eph *RtcmEph) (X float64, Y float64, Z float64) {
	//var gmearth float64
	//var earth_rotate float64
	//if (eph.Sys==SYS_GPS){
	//	gmearth = 3.986005E14
	//	earth_rotate = 7.2921151467E-5
	//}

	return 0, 0, 0
}

/**
 * 替换成新的TypeDescriptor
 */
func Rtcm1033ReplReceiverTypeDescriptor(rtcmWholeBuf []byte, new string) []byte {
	rec := &Gnss1033Rec{}
	if DecodeRtcm1033(rec, rtcmWholeBuf, 24) > 0 {
		var rtcm RtcmEncoder
		rec.ReceiverTypeDescriptor = new
		bitN := EncodeRtcm1033(rec, rtcm.RtcmBodyBuf(), 0)
		return rtcm.EncodeRtcmRawBuf(bitN / 8)
	} else {
		return nil
	}
}

/*
信号freq
*/
func GetSigFreq(satsys byte, sigv byte, prn byte) float64 {
	if satsys == SYS_GPS || satsys == SYS_SBS {
		if sigv <= 1 {
			return 0.0
		} else if sigv >= 2 && sigv <= 4 { //2..4   L1
			return FREQ_GPS_L1
		} else if sigv == 8 { //2..4   L2
			return FREQ_GPS_2
		} else if sigv >= 9 && sigv <= 10 { //9..10 L2
			return FREQ_GPS_2
		} else if sigv >= 15 && sigv <= 17 { //15..17 L2
			return FREQ_GPS_2
		} else if sigv >= 22 && sigv <= 24 { //22..24 L5
			return FREQ_GPS_5
		} else if sigv >= 30 && sigv <= 32 { //30..32 L1
			return FREQ_GPS_L1
		}
		return 0.0
	} else if satsys == SYS_BD {
		if sigv <= 1 { // 未知
			return 0.0
		} else if sigv >= 2 && sigv <= 4 { //2..4   I, Q, I+Q  B1(一期)
			return FREQ_BD_B1I
		} else if sigv >= 8 && sigv <= 10 { // 8.. 10
			return FREQ_BD_B3I
		} else if sigv >= 14 && sigv <= 16 { // 14.. 16
			return FREQ_BD_B2I
		} else if sigv >= 22 && sigv <= 24 { // 22.. 24
			return FREQ_BD_B2A
		} else if sigv == 25 {
			return FREQ_BD_B2B
		} else if sigv >= 30 && sigv <= 32 { // 30,31,32
			return FREQ_BD_B1C
		} else if sigv >= 17 && sigv <= 19 { // igmas: 17,18,19
			return FREQ_BD_B2A
		} else if sigv >= 23 && sigv <= 25 { // igmas
			return FREQ_BD_B2AB
		} else if sigv >= 26 && sigv <= 31 { // igmas
			return FREQ_BD_B1C
		} else if sigv == 7 { // UB4B0(igmas早期版本)
			return FREQ_BD_B1C
		}
	} else if satsys == SYS_GAL {
		if sigv <= 1 {
			return 0.0
		} else if sigv >= 2 && sigv <= 6 { //2..6   E1
			return FREQ_E1
		} else if sigv >= 8 && sigv <= 12 { //8..12 E6
			return FREQ_E6
		} else if sigv >= 14 && sigv <= 16 { //14..16 E5B
			return FREQ_E5B
		} else if sigv >= 18 && sigv <= 20 { //18..20 E5(A+B)
			return FREQ_E5AB
		} else if sigv >= 22 && sigv <= 24 { //22..24 E5A
			return FREQ_E5A
		}
		return 0.0
	} else if satsys == SYS_GLO {
		idx := int(prn) - 1
		if idx < 0 || idx >= len(GLO_FRQ) {
			return 0
		}
		fn := GLO_FRQ[idx]
		if sigv <= 1 {
			return 0.0
		} else if sigv >= 2 && sigv <= 3 { //2..3   G1
			return (FREQ1_GLO + DFRQ1_GLO*fn)
		} else if sigv >= 8 && sigv <= 9 { //8..9  G2
			return (FREQ2_GLO + DFRQ2_GLO*fn)
		}
		return 0.0
	} else if satsys == SYS_QZS {
		if sigv <= 1 {
			return 0.0
		} else if sigv >= 2 && sigv <= 4 { //2..4   L1
			return FREQ_GPS_L1
		} else if sigv <= 8 {
			return 0
		} else if sigv >= 9 && sigv <= 11 { // LEX L6 1278.75
			return FREQ_GPS_6
		} else if sigv >= 15 && sigv <= 17 { //15..17 L2
			return FREQ_GPS_2
		} else if sigv >= 22 && sigv <= 24 { //22..24 L5
			return FREQ_GPS_5
		} else if sigv >= 30 && sigv <= 32 { //30..32 L1
			return FREQ_GPS_L1
		}
	} else {
		return 0.0
	}

	return 0.0
}

// / 信号波长
func SigWaveLen(satsys byte, sigv byte, prn byte) float64 {
	freq := GetSigFreq(satsys, sigv, prn)
	if freq > 0 {
		return LIGHT_SPEED / freq
	}
	//if satsys == SYS_GPS || satsys == SYS_SBS {
	//	if sigv <= 1 {
	//		return 0.0
	//	} else if sigv >= 2 && sigv <= 4 { //2..4   L1
	//		return LIGHT_SPEED / FREQ_GPS_L1
	//	} else if sigv == 8 { //2..4   L2
	//		return LIGHT_SPEED / FREQ_GPS_2
	//	} else if sigv >= 9 && sigv <= 10 { //9..10 L2
	//		return LIGHT_SPEED / FREQ_GPS_2
	//	} else if sigv >= 15 && sigv <= 17 { //15..17 L2
	//		return LIGHT_SPEED / FREQ_GPS_2
	//	} else if sigv >= 22 && sigv <= 24 { //22..24 L5
	//		return LIGHT_SPEED / FREQ_GPS_5
	//	} else if sigv >= 30 && sigv <= 32 { //30..32 L1
	//		return LIGHT_SPEED / FREQ_GPS_L1
	//	}
	//	return 0.0
	//} else if satsys == SYS_BD {
	//	/**
	//	hc:
	//	我们北斗B1C放在频点索引29上，B2a放在频点索引21上
	//	"我们收到的好像没有29, 21"
	//	------
	//	这个我们是从0开始计算，您是1，是一样的
	//
	//	您好，给我回复，我们是1D跟5C
	//	*/
	//	if sigv <= 1 { // 未知
	//		return 0.0
	//	} else if sigv >= 2 && sigv <= 4 { //2..4   I, Q, I+Q  B1(一期)
	//		return LIGHT_SPEED / FREQ_BD_B1I
	//	} else if sigv <= 7 { // 5..7   B1C
	//		return LIGHT_SPEED / FREQ_BD_B1C
	//	} else if sigv <= 10 { // 8...10
	//		return LIGHT_SPEED / FREQ_BD_B3I
	//	} else if sigv <= 13 { // 11..13
	//		return LIGHT_SPEED / FREQ_BD_B3C
	//	} else if sigv <= 16 { // 14..16
	//		return LIGHT_SPEED / FREQ_BD_B2I
	//	} else if sigv <= 19 { // 17..19
	//		return LIGHT_SPEED / FREQ_BD_B2A
	//	} else if sigv == 22 { // 华测VRS B2a放在频点索引22上 5D
	//		return LIGHT_SPEED / FREQ_BD_B2A
	//	} else if sigv == 23 { // 千寻VRS (23频点) B2a
	//		return LIGHT_SPEED / FREQ_BD_B2A
	//	} else if sigv <= 22 { // 20..22
	//		return LIGHT_SPEED / FREQ_BD_B2B
	//	} else if sigv <= 25 { // 23..25
	//		return LIGHT_SPEED / FREQ_BD_B2A
	//	} else if sigv >= 26 && sigv <= 31 { // 26..31
	//		return LIGHT_SPEED / FREQ_BD_B1C
	//	}
	//} else if satsys == SYS_GAL {
	//	if sigv <= 1 {
	//		return 0.0
	//	} else if sigv >= 2 && sigv <= 6 { //2..6   E1
	//		return LIGHT_SPEED / FREQ_E1
	//	} else if sigv >= 8 && sigv <= 12 { //8..12 E6
	//		return LIGHT_SPEED / FREQ_E6
	//	} else if sigv >= 14 && sigv <= 16 { //14..16 E5B
	//		return LIGHT_SPEED / FREQ_E5B
	//	} else if sigv >= 18 && sigv <= 20 { //18..20 E5(A+B)
	//		return LIGHT_SPEED / FREQ_E5AB
	//	} else if sigv >= 22 && sigv <= 24 { //22..24 E5A
	//		return LIGHT_SPEED / FREQ_E5A
	//	}
	//	return 0.0
	//} else if satsys == SYS_GLO {
	//	idx := int(prn) - 1
	//	if idx < 0 || idx >= len(GLO_FRQ) {
	//		return 0
	//	}
	//
	//	fn := GLO_FRQ[idx]
	//	if sigv <= 1 {
	//		return 0.0
	//	} else if sigv >= 2 && sigv <= 3 { //2..3   G1
	//		return LIGHT_SPEED / (FREQ1_GLO + DFRQ1_GLO*fn)
	//	} else if sigv >= 8 && sigv <= 9 { //8..9  G2
	//		return LIGHT_SPEED / (FREQ2_GLO + DFRQ2_GLO*fn)
	//	}
	//	return 0.0
	//} else if satsys == SYS_QZS {
	//	//if sigtype == SIG_TYPE_QZS_L1{ // L1
	//	//	return LIGHT_SPEED / FREQ_GPS_L1
	//	//}else if sigtype == SIG_TYPE_QZS_L2 {
	//	//	return LIGHT_SPEED / FREQ_GPS_2
	//	//} else if sigtype == SIG_TYPE_QZS_L5{
	//	//	return LIGHT_SPEED / FREQ_GPS_5
	//	//}
	//	//YOBS_QZS_SIGN_MAP = []string{"",
	//	//	"1C", "", "", // 2, 3, 4    L1
	//	//	"", "", "", "", // 5,6,7, 8
	//	//	"6S", "6L", "6X", // 9, 10 11   LEX L6 1278.75
	//	//	"", "", "", // 12-14
	//	//	"2S", "2L", "2X", // 15-17     L2
	//	//	"", "", "", "", // 18-21
	//	//	"5I", "5Q", "5X", // 22-24     L5
	//	//	"", "", "", "", "", // 25 -29
	//	//	"1S", "1L", "1X", //30-32    // L1
	//	//}
	//
	//	if sigv <= 1 {
	//		return 0.0
	//	} else if sigv >= 2 && sigv <= 4 { //2..4   L1
	//		return LIGHT_SPEED / FREQ_GPS_L1
	//	} else if sigv <= 8 {
	//		return 0
	//	} else if sigv >= 9 && sigv <= 11 { // LEX L6 1278.75
	//		return LIGHT_SPEED / FREQ_GPS_6
	//	} else if sigv >= 15 && sigv <= 17 { //15..17 L2
	//		return LIGHT_SPEED / FREQ_GPS_2
	//	} else if sigv >= 22 && sigv <= 24 { //22..24 L5
	//		return LIGHT_SPEED / FREQ_GPS_5
	//	} else if sigv >= 30 && sigv <= 32 { //30..32 L1
	//		return LIGHT_SPEED / FREQ_GPS_L1
	//	}
	//} else {
	//	return 0.0
	//}

	return 0.0
}

func SprintRtcmMsm(preLine string, msm *RtcmMsm) string {

	var k byte = 0
	h := msm.Header
	l := len(msm.ObsV)
	str := ""

	for i := 0; i < l; i++ {
		v := msm.ObsV[i]

		prn := v.Prn //gnss.GetPrnStrCode(sObs.Sys, byte(v.Prn));

		signStr := ""

		for k = 0; k < h.SigN; k++ {
			sigi := h.Sigs[k]
			if sigi <= MAX_SIGNA_N {
				sSign := GetSignCode(sigi, msm.Sys)
				if v.PseV[k] > 0 {
					signStr += fmt.Sprintf("C%s %s L%s %s 000 S%s %s ", sSign, RinexFormatFloat(v.PseV[k]), sSign, RinexFormatFloat(v.L[k]),
						sSign, RinexFormatFloat(v.Snr[k]))
				}
			}
		}

		if len(signStr) > 0 {
			str += fmt.Sprintf("%s%s %s\n", preLine, prn, signStr)
		}
	}

	return str

}

const (
	// fine伪距的分辨率 GNSS signal fine Pseudorange msm4
	Resolution_PsRng0_DF400 float64 = P2_24 * RANGE_MS

	// GNSS signal fine Pseudorange with extended resolution msm7
	Resolution_PsRng0_DF405 float64 = P2_29 * RANGE_MS

	// fine 相位的分辨率 GNSS signal fine Phaserange data msm4
	// ±(2^ –8 - 2 ^ –29) ms (Approx: ±1171 m)
	Resolution_PhRng0_DF401 float64 = P2_29 * RANGE_MS

	// fine 相位的分辨率 GNSS signal fine Phaserange data msm4
	// ±(2^ –8 - 2 ^ –31) ms (Approx: ±1171 m)
	Resolution_PhRng0_DF406 float64 = P2_31 * RANGE_MS
)

func PsRng0To1_DF400(raw0 int16) float64 {
	v := float64(raw0) * Resolution_PsRng0_DF400
	return v
}

/*
*
(Approx: ±292 m)
*/
func PsRng1To0_DF400(raw1 float64) int16 {
	if math.Abs(raw1) > 293 {
		panic(fmt.Sprintf("%.4f out of psrng(Approx: ±292 m) ref: RTCM DF400", raw1))
	}
	return int16(math.Round(raw1 / Resolution_PsRng0_DF400))
}

/*
*
(Approx: ±292 m)
*/
func PsRng1To0_DF405(raw1 float64) int32 {
	if math.Abs(raw1) > 293 {
		panic(fmt.Sprintf("%.4f out of psrng(Approx: ±292 m) ref: RTCM DF405", raw1))
	}
	return int32(math.Round(raw1 / Resolution_PsRng0_DF405))
}

func PhRng0To1_DF401(raw0 int32) float64 {
	v := float64(raw0) * Resolution_PhRng0_DF401
	return v
}

// (Approx: ±1171 m)
func PhRng1To0_DF401(raw1 float64) int32 {
	if raw1 < -1171 || raw1 > 1171 {
		panic(fmt.Sprintf("%.4f out of phrng(Approx: ±1171 m) ref: RTCM DF401", raw1))
	}
	return int32(math.Round(raw1 / Resolution_PhRng0_DF401))
}

// (Approx: ±1171 m)
func PhRng1To0_DF406(raw1 float64) int32 {
	if raw1 < -1171 || raw1 > 1171 {
		panic(fmt.Sprintf("%.4f out of phrng(Approx: ±1171 m) ref: RTCM DF406", raw1))
	}
	return int32(math.Round(raw1 / Resolution_PhRng0_DF406))
}

func CheckRougRngFail(rr float64, lst []float64) (failidx int) {
	for i := 0; i < len(lst); i++ {
		if math.Abs(lst[i]-rr) > 293 {
			return i
		}
	}
	return -1
}

/*
*
计算卫星的粗略范围值, 如果为0 说明无法计算, 该卫星不可用
*/
func CalcuRoughRngBySlice(lst sort.Float64Slice) float64 {
	l := lst.Len()
	if l == 0 {
		return 0
	}
	if l == 1 {
		return math.Round(lst[0]/P2_10_RANGE_MS) * P2_10_RANGE_MS
	} else if l == 2 {
		if math.Abs(lst[0]-lst[1]) > 293 { // 两个伪距之间相差大于292 忽略掉
			return 0
		} else {
			lst.Sort()
			v1 := math.Ceil(lst[0]/P2_10_RANGE_MS) * P2_10_RANGE_MS
			if v1-lst[0] > 232 {
				v1 -= P2_10_RANGE_MS
				if math.Abs(v1-lst[1]) > 232 {
					return 0
				}
				return v1
			} else {
				return v1
			}
		}
	} else {
		// 取中位值
		lst.Sort()
		m := l / 2
		return math.Round(lst[m]/P2_10_RANGE_MS) * P2_10_RANGE_MS
	}
}

/*
*

	比较两个Msm7 Buf 是否一致
*/
func CompareRtcmMsm7Buf(buf0 []byte, buf1 []byte, ignoreSmoothingflag byte) bool {
	var emptyBuf0 []byte = make([]byte, 0, 1024)
	var emptyBuf1 []byte = make([]byte, 0, 1024)
	if ignoreSmoothingflag == 1 {
		buf0 = append(emptyBuf0, buf0...)
		buf1 = append(emptyBuf1, buf1...)
		buf0 = RtcmMsmBufEmptyData(buf0, 1, 1, 1)
		buf1 = RtcmMsmBufEmptyData(buf1, 1, 1, 1)
	}
	fmt.Println(utils.BufToHexStr(buf0, 0, ""))
	fmt.Println(utils.BufToHexStr(buf1, 0, ""))
	return bytes.Compare(buf0, buf1) == 0
}

func RtcmMsmBufIsEnd(rtcmPackBuf []byte, multimsgflag, clockflag, smoothingflag byte) bool {
	bitN := 24 + 12 + 12 + 30 //Multiple Message Bit
	flag := GetBitU8(rtcmPackBuf, bitN, 1)
	return flag == 1
}

func RtcmMsmBufEmptyData(rtcmPackBuf []byte, multimsgflag, clockflag, smoothingflag byte) (newBuf []byte) {
	bitN := 24 + 12 + 12 + 30 //Multiple Message Bit
	if multimsgflag == 1 {
		SetBitU8(rtcmPackBuf, bitN, 1, 0)
	}
	bitN += 1

	bitN = bitN + 3 // skip DF409

	// DF001
	SetBitU8(rtcmPackBuf, bitN, 7, 0)
	bitN += 7

	if clockflag == 1 {
		// Clock Steering Indicator
		SetBitU8(rtcmPackBuf, bitN, 2, 0)
		SetBitU8(rtcmPackBuf, bitN+2, 2, 0)
	}
	bitN += 4 // skip DF411, DF412

	if smoothingflag == 1 {
		SetBitU8(rtcmPackBuf, bitN, 1, 0)
		SetBitU8(rtcmPackBuf, bitN+1, 3, 0)
	}

	return rtcmPackBuf
}

func RtcmMsmBufSetStationId(rtcmBuf []byte, staid uint16) {
	SetBitU16(rtcmBuf, 24+12, 12, staid)
}

func RtcmMsm7BufEmptyBodyData(rtcmPackBuf []byte, exflag byte) (newBuf []byte) {
	_, _, satN, _, cellN := ExtractRtcmMsmHeadInfo(rtcmPackBuf)
	bitN := 24 + 169 + int(cellN)

	// skip Satellite rough ranges DF397
	bitN += int(satN) * 8

	// Extended Satellite Information
	if exflag == 1 {
		for i := U8_0; i < satN; i++ {
			SetBitU8(rtcmPackBuf, bitN, 4, 0)
			bitN += 4
		}
	}
	return rtcmPackBuf
}

// set定msm的结束标记
// 0:表示正常结束,
// 1:表示未结束
// rtcmBuf 完整的rtcmmsm消息
func SetRtcmMsmSynchronousFlag(rtcmMsmBuf []byte, flag byte) {
	gobase.SetBitU8(rtcmMsmBuf, 24+12+12+30, 1, flag)
}

// 设定最后一个msmbuf的sync标记
func SetRtcmMsmSyncFlag(msmbuflst []byte) (changed bool) {
	RangeRtcmMsmBuf(msmbuflst, 0, func(msgid int, rtcmbuf []byte, endflag bool) bool {
		syncflag := GetRtcmMsmSynchronousFlag(rtcmbuf, 24)
		if !endflag {
			if syncflag == 0 {
				SetRtcmMsmSynchronousFlag(rtcmbuf, 1)
				RtcmPackBufReCrc(rtcmbuf)
				changed = true
			}
		} else {
			if syncflag == 1 {
				SetRtcmMsmSynchronousFlag(rtcmbuf, 0)
				RtcmPackBufReCrc(rtcmbuf)
				changed = true
			}
		}
		return true
	})
	return
}

// 循环所有的msm buf
// fn中不要改变rtcmbuf的大小.
func RangeRtcmMsmBuf(rtcmBuflst []byte, crcflag int, fn func(msgid int, rtcmbuf []byte, endflag bool) bool) {
	var startidx int = 0
	var msgl int = 0
	var preMsgBuf []byte
	var preMsgId int
	for i := 0; i < len(rtcmBuflst); {

		if rtcmBuflst[i] == 0xD3 {
			startidx = i
			u16 := binary.BigEndian.Uint16(rtcmBuflst[startidx+1:])
			msgl = int(u16) & 0x3FF                      // 0x3FF 1111111111  1023
			if (startidx + msgl + 6) > len(rtcmBuflst) { // 长度不够
				i++ // 跳过
				continue
			}

			rtcmbuf := rtcmBuflst[startidx : startidx+msgl+6]
			if crcflag == 1 {
				crc1 := gobase.CRC24QBuf(rtcmbuf[:len(rtcmbuf)-3])
				crc := gobase.GetBitU(rtcmbuf[len(rtcmbuf)-3:], 0, 24)
				if crc1 != crc {
					i++ // 跳过
					continue
				}
			}

			msgid := int(gobase.GetBitU(rtcmbuf, 24, 12))
			if RtcmIsObsMsm(msgid) {
				if len(preMsgBuf) > 0 {
					if !fn(preMsgId, preMsgBuf, false) {
						break
					}
				}
				preMsgBuf = rtcmbuf
				preMsgId = msgid
			}
			i += msgl + 6
		} else {
			i++
		}
	}

	// 最后一个
	if len(preMsgBuf) > 0 {
		fn(preMsgId, preMsgBuf, true)
	}
}

// 会直接改变传入的rtcmPackBuf的值
func RtcmPackBufReCrc(rtcmPackBuf []byte) (newBuf []byte) {
	crcByteN := len(rtcmPackBuf) - 3
	crcBuf := rtcmPackBuf[:crcByteN] // 3:同步标记 + 保留位 + 长度（RTCM消息体）(10bit)
	crc1 := utils.CRC24QBuf(crcBuf)
	SetBitU32(rtcmPackBuf[crcByteN:], 0, 24, crc1)
	return rtcmPackBuf
}

func EncodeRtcmPack(msgBuf []byte) (rtcmBuf []byte, err error) {
	rtcmBuf = append(rtcmBuf, 0xD3, 0, 0)
	l := uint16(len(msgBuf))
	if l > 1023 {
		err = fmt.Errorf("rtcm body l:%d > 1023", l)
		return
	}
	SetBitU16(rtcmBuf, 14, 10, l)
	rtcmBuf = append(rtcmBuf, msgBuf...)
	crc1 := utils.CRC24QBuf(rtcmBuf)
	rtcmBuf = append(rtcmBuf, 0, 0, 0)
	SetBitU32(rtcmBuf[l+3:], 0, 24, crc1)
	return
}

func ExtractRtcmMsmHeadInfo(rtcmPackBuf []byte) (satMask uint64, sigmask uint32, satN byte, sigN, cellN byte) {
	bitsTotalN := len(rtcmPackBuf) * 8
	if bitsTotalN < 169 {
		return
	}

	buf := rtcmPackBuf[3:]
	bitN := 73
	{
		u64 := GetBitU64(buf, bitN, 64)
		bitN += 64
		satMask = u64
		satN = utils.GetBitOnU64(u64)
	}

	{ // 信号
		u32 := GetBitU(buf, bitN, 32)
		bitN += 32
		sigmask = u32
		sigN = utils.GetBitOnU32(u32)
	}

	// 169+cellN (cellN < 64)
	cellN = satN * sigN

	return
}
